科研速递 | 林天麟教授团队在TMECH上发表论文，解决机器人初始位姿估计问题

机器人姿态估计是机器人系统的基本任务之一。在没有外部信息的情况下，机器人无法获知自身相对于全局坐标系的绝对姿态。如何使用局部测量确定机器人的相对姿态是一个亟待解决的课题。

日前，香港中文大学（深圳）理工学院林天麟教授（兼任深圳市人工智能与机器人研究院智能机器人中心主任）发表了论文“Asymptotically Efficient Estimator for Range-based Robot Relative Localization”。该论文使用机器人里程计以及超宽带测距信息，估计参考机器人坐标系下的其他机器人位置和方向；并从理论上证明所提出的方法为渐进有效估计器。

1

摘要概述

本文结合机器人里程计和超宽带雷达测距信息估计两个移动机器人的相对位姿。文中提出了一种鲁棒加权半正定松弛方法，用于处理存在距离测量误差和机器人状态转换误差的相对定位问题。理论分析和模拟结果表明，当测量噪声和里程计不确定性符合高斯分布的情况下，文中所提出的加权半正定松弛方法为渐进无偏估计器，且方差可以达到克劳美罗下界。在使用相对廉价的超宽带雷达以及IMU里程计的情况下，机器人相对方向和位置估计的均方根误差为3.97度和0.22米。

2

期刊简介

《IEEE/ASME机电一体化汇刊》（IEEE/ASME Transactions on Mechatronics，简称TMECH）由美国电子电气工程师协会（IEEE）和美国机械工程师协会（ASME）共同主办，是机电工程领域的顶级期刊。

3

背景简介

由于机器人相对位姿对目标拦截/包围、机器人集群、和无人机系统等方面至关重要，近年来，机器人相对定位的研究受到越来越多的关注。超宽带测距具有低成本、低功耗、高精度和能够在非视距环境下应用等优点。此外，通过超宽带模块收集的距离测量值无需进行数据关联。同时，机器人配备有惯性测量单元或双轮差动式里程计，用于测量机器人的自我运动。超宽带模块和里程计的轻量化和经济性等优点使在无GPS环境中部署一组小型、廉价的机器人成为可能。

传感器网络刚体定位与机器人相对定位问题密切相关。刚体定位使用刚体上的传感器与已知锚点之间的距离或角度测量值即时估计刚体的全局姿态。然而，超宽带模块测距的平均噪声约为0.1米，与许多模块化机器人的尺寸相当。受小型机器人的传感器阵列孔径的限制，使用单帧数据定位机器人的相对姿态失败率较高。使用多帧里程计和测距信息可以节省实施成本，并显著扩大传感器阵列孔径。

受到刚体定位方法的启发，本研究利用多帧里程计信息和距离测量值来估计机器人的相对位姿。现有研究大多假设里程计无噪声或忽略机器人位置误差的累积特性。本研究将由状态转移误差导致的超宽带雷达位置误差与距离测量噪声合并建模，并提出了一种估计机器人相对位姿的加权半定松弛方法（WSDR）。此外，以往关于机器人相对定位的研究缺乏对估计器统计性能的理论分析。本文推导了机器人相对位姿的克劳美罗下界，并从理论上和仿真上验证了文中所提出的估计器为渐进有效估计器；即方差与克劳美罗下界重叠的无偏估计器。

4

本文方法

本文首先通过状态转移误差的协方差矩阵推导出超宽带模组位置误差的协方差矩阵，并通过加权矩阵合并超宽带模组位置误差以及距离测量噪声的信息；接下来使用多帧机器人里程计和超宽带雷达距离测量值估计在参考机器人坐标系下其他机器人的相对位置和方向；最后将WSDR所估计的机器人初始相对位姿带入基于滤波器的相对位姿追踪算法中，以获得后续时刻的机器人相对位姿。

定位场景

机器人1和机器人2在第m帧的位置可表示为：

和是时刻机器人1和2的位姿，和是时刻机器人1和2的状态转移向量。和的定义以及具体推导细节请参考论文全文。

不失一般性的，本文选择机器人1作为参考机器人。和表示时刻机器人2在机器人1坐标系中的方向和位置。结合前一时刻机器人相对位姿以及当前时刻的状态转移向量，当前时刻机器人相对位姿可表示为：

因此，只要已知初始时刻机器人相对位姿和后续时刻状态转移向量，可以计算任意时刻机器人相对位姿。状态转移向量可以由机器人里程计直接测量，为了计算初始时刻机器人相对位姿，本文使用超宽带雷达测距信息:

和即机器人初始时刻位姿。

图一：机器人相对定位的场景。灰色和白色三角形代表机器人1和机器人2。正方形表示超宽带模块。本文的目标是确定初始时刻机器人2在机器人1坐标系下的方向和位置，即和。

本文用“”表示“”的真实值；然而实际应用中，超宽带距离测量值和机器人里程计都含有测量噪声。本文假设超宽带测距噪声符合高斯分布且协方差矩阵为；任意两帧之间的状态转换误差为独立且同分布且符合协方差矩阵为和的高斯分布。状态转移误差随时间累积产生了机器人位置误差和超宽带雷达位置误差。因此，超宽带雷达位置误差的协方差矩阵可以表示为

式(5)和（6）的符号含义以及具体细节请参考论文全文。

相对位姿估计

对式（4）左右两边取平方并展开可以得到：

在里程计的距离测量值均不含噪声时，残差为零。使用可以将旋转矩阵向量化，其中且。

定义未知量为增广向量。将所有测量值对应的等式写成矩阵形式可以得到

其中

，和为超宽带雷达测量噪声和位置噪声的系数矩阵，具体细节请参考论文全文。定义并假设的秩可以大于1；机器人相对位姿估计问题可以转化成最小化含约束的代价函数的问题，即

其中，W为加权矩阵

由于2D机器人相对位姿的自由度为3，而增广未知量是的向量；因此中含有4个冗余变量。，，和由增广向量元素之间的约束决定。本文使用MATLAB CVX工具包[1]最小化(11)并求解。利用可以计算机器人初始相对位姿：

5

仿真实验

本文从理论性能分析和仿真实验两方面验证了所提出的估计器为渐进有效估计器。仿真结果表明，WSDR估计器产生的均方根误差较其他方法最低并能达到克劳美罗下界。而且增加使用的数据帧数可以减少估计器产生的均方根误差并增强WSDR对测量噪声及里程计误差的鲁棒性。

图二：超宽带测距误差增加时机器人初始相对位姿均方根误差（左）；数据帧数增加时机器人初始相对位姿均方根误差（右）

6

实机实验

在实机实验中研究团队使用了SPARK机器人以及Decawave DWM1000超宽带模块验证文中估计器的性能。实验使用运动捕捉系统OptiTrack捕捉四个机器人的运动轨迹和方向真实值。香港中文大学（深圳）学生林沐晗和谢馨逸协助完成了实机实验。实验结果表明，相较其他估计器，使用 WSDR估计得到的机器人初始位姿与通过运动捕捉系统获取的真实值最为贴近。当需要追踪机器人相对位姿时，本文利用WSDR估计值来初始化机器人相对位姿；并利用式（2）和（3）中的状态转移模型更新机器人相对位姿。当使用的超宽带测距数据超过200帧时，卡尔曼滤波器和滑动窗口滤波器可显著降低机器人相对位姿追踪的累积误差。

图三：实机实验性能（左）；机器人相对位姿追踪均方根误差（右）

7

小结

本文讨论了在机器人里程计存在累计误差时，利用超宽带模块测距数据对机器人初始相对位姿进行估计的问题。性能分析和仿真实验表明文中所提出的WSDR估计器在噪声功率不显著时是无偏的，且方差可以达到克劳美罗下界。实机实验验证了WSDR的准确性和鲁棒性。此工作未来还可以扩展并应用于3D环境下多机器人相对定位和追踪问题。

8

作者简介

本文第一作者：王悦，原深圳市人工智能与机器人研究院副研究员，研究方向包括统计信号处理，估计理论，目标定位。以第一作者身份在IEEE TSP, IEEE TWC，IEEE TMECH等期刊及会议发表论文10余篇。

本文第二作者：林沐晗，香港中文大学（深圳）理工学院23届计算机工程本科生，现于卡耐基梅隆大学攻读机器人学研究硕士学位，师从Katia Sycara教授。先后任深圳市人工智能与机器人研究院、卡耐基梅隆大学机器人学院学生研究员。主要研究方向为多机协作。

本文第三作者：谢馨逸，目前是香港中文大学(深圳)的本科生，她曾担任深圳市人工智能与机器人研究院的研究助理。研究兴趣主要集中在计算机视觉和机器人领域。

本文通讯作者：林天麟，香港中文大学（深圳）助理教授，博士生导师，广东省杰出青年基金获得者，IEEE高级会员，担任机器人与智能制造国家地方联合工程实验室常务副主任、深圳市人工智能与机器人研究院（AIRS）智能机器人中心主任。师从徐扬生院士，分别于2006年和2010年在香港中文大学获得一等荣誉学士学位和博士学位。研究方向包括模块化机器人、多机器人系统及特种机器人等。林教授至今在T-RO、TPAMI、TIP、TMECH、ICRA、IROS等知名期刊及会议发表论文100余篇，其中2023年以独立通讯作者身份在T-RO发表论文5篇；获TMECH年度最佳论文奖，IROS机器人机构设计最佳论文奖；作为项目负责人，主持国家自然科学基金委、国家科技部、广东省科技厅、深圳市科创委的多项科研项目；相关研究成果被路透社、探索频道、日本放送协会NHK、IEEE Spectrum等众多国际知名媒体报导。

9

团队介绍

Freeform Robotics团队成立于2019年，由香港中文大学（深圳）林天麟教授领导，旨在研究多机器人系统自由组成各种形态以解决不可预知问题的关键技术，通过简单智能体的集群实现复杂的智能群体行为，让机器人系统拥有可复用、自由构型、可拓展、故障自修复等通用特性，为机器人设计领域创造出一种全新切实可行的实现形态。

团队长期从事机器人和人工智能研究，开发了十余种机器人和智能系统；承担来自国家自然科学基金委、国家科技部、广东省科技厅、深圳市科创委等多项重要研究项目；科研成果发表于T-RO、TPAMI、TIP、TMECH、ICRA、IROS等机器人与人工智能领域的国际期刊和会议上，其中2023年在T-RO发表论文5篇。关于FreeBOT的研究成果获2020年IROS机器人机构与设计最佳论文奖，IEEE Spectrum、日本放送协会NHK和Engadget 等多家国际知名媒体对其进行了广泛报道。

完整的结果和分析参见全文：

https://ieeexplore.ieee.org/document/10142210

[1] M. Grant and S. Boyd. CVX: MATLAB Software for Disciplined Convex Programming, Version 1.21. [Online]. Available: http://cvxr.com/cvx

供稿 | 林天麟教授团队

END

点击以下链接，进入理工时刻：